#include "ms575_algbackss1ss2smallcircle.h"
using namespace MeasurE575;

AlgBackSS1SS2SmallCircle::AlgBackSS1SS2SmallCircle(int i):AlgBase(i)
{

}

void AlgBackSS1SS2SmallCircle::operator()(cv::Mat &srcImg,
                                          int id,
                                          std::map<string, float> &measRes,
                                          int fixtureId)
{
 //back SS1->31, SS2-> 32

    std::vector<int> paras19,paras20;
    for(int j=0;j< m_dataAllVec[31].m_edgeParam.cols;j++)
    {
        paras19.push_back(m_dataAllVec[31].m_edgeParam.at<int>(0,j));
        paras20.push_back(m_dataAllVec[32].m_edgeParam.at<int>(1,j));
    }
    std::vector<float> resultSS1,resultSS2;
    std::vector<cv::Rect> rangSS1Vec,rangSS2Vec;

    {
        TemplateMatch(srcImg,m_dataAllVec[31],rangSS1Vec);
        edgedetect obj;
        obj.NewSmallCircle(srcImg(rangSS1Vec[0]),paras19,resultSS1);
        resultSS1[0]+=rangSS1Vec[0].x;
        resultSS1[1]+=rangSS1Vec[0].y;
    }

    {
        TemplateMatch(srcImg,m_dataAllVec[32],rangSS2Vec);
        edgedetect obj;
        obj.NewSmallCircle(srcImg(rangSS2Vec[0]),paras20,resultSS2);
        resultSS2[0]+=rangSS2Vec[0].x;
        resultSS2[1]+=rangSS2Vec[0].y;
    }

    std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
    while (!AlgBase::flagVec[id])
    {
        (*AlgBase::cvarptrVec[id]).wait(lock);
    }

    omp_set_num_threads(2);
#pragma omp parallel sections
    {
        #pragma omp section
        {
            cv::Point2f ptsOut, ptsIn(resultSS1[0],resultSS1[1]);
            CoordinateTransformationRelateToDot(datumBackVec[id],  ptsIn , ptsOut);
            resultSS1[0]=ptsOut.x;
            resultSS1[1]=ptsOut.y;
         }
        #pragma omp section
          {
            cv::Point2f ptsOut,ptsIn(resultSS2[0],resultSS2[1]);
            CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn,ptsOut);
            resultSS2[0]=ptsOut.x;
            resultSS2[1]=ptsOut.y;
          }
     }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    measRes.insert(std::make_pair(string("FAI82-SS2-Y-distance_pixel"), -resultSS2[1]));
    float fai82ss2y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI82-SS2-Y-distance")], -resultSS2[1]);
    measRes.insert(std::make_pair(string("FAI82-SS2-Y-distance_real"), fai82ss2y));

    measRes.insert(std::make_pair(string("FAI88-SS2-X-distance_pixel"), -resultSS2[0]));
    float fai82ss2x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI88-SS2-X-distance")], -resultSS2[0]);
    measRes.insert(std::make_pair(string("FAI88-SS2-X-distance_real"), fai82ss2x));

    measRes.insert(std::make_pair(string("S1-diam_pixel"), resultSS2[2]*2));
    float s1d = CalcMeasureDistance(m_phypixpara[prefix + std::string("S1-diam")], resultSS2[2]*2);
    measRes.insert(std::make_pair(string("S1-diam_real"), s1d));

    measRes.insert(std::make_pair(string("FAI172-SS1-Y-distance_pixel"), -resultSS1[1]));
    float fai172ss1y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI172-SS1-Y-distance")], -resultSS1[1]);
    measRes.insert(std::make_pair(string("FAI172-SS1-Y-distance_real"), fai172ss1y));

    measRes.insert(std::make_pair(string("FAI171-SS1-X-distance_pixel"), -resultSS1[0]));
    float fai171ss1x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI171-SS1-X-distance")], -resultSS1[0]);
    measRes.insert(std::make_pair(string("FAI171-SS1-X-distance_real"), fai171ss1x));

    measRes.insert(std::make_pair(string("S2-diam_pixel"), resultSS1[2]*2));
    float s2d = CalcMeasureDistance(m_phypixpara[prefix + std::string("S2-diam")], resultSS1[2]*2);
    measRes.insert(std::make_pair(string("S2-diam_real"), s2d));
}

